/**
  ******************************************************************************
  * @file    main.c
  * @author  Vsion yang
  * @version V1.0.0
  * @date    2019.10.31
  * @brief   
  ******************************************************************************
**/
#include "main.h"
#include "oled.h"
#include "display.h"
#include "usart.h"

//sin cos  
const int16_t sin_1[] = 
{
    0,      1,      3,      4,      6,      7,      9,     10,     12,     14,     15,     17,     18,     20,     21,     23,  
   25,     26,     28,     29,     31,     32,     34,     36,     37,     39,     40,     42,     43,     45,     47,     48,  
   50,     51,     53,     54,     56,     58,     59,     61,     62,     64,     65,     67,     69,     70,     72,     73,  
   75,     76,     78,     80,     81,     83,     84,     86,     87,     89,     90,     92,     94,     95,     97,     98,  
  100,    101,    103,    105,    106,    108,    109,    111,    112,    114,    115,    117,    119,    120,    122,    123,  
  125,    126,    128,    130,    131,    133,    134,    136,    137,    139,    140,    142,    144,    145,    147,    148,  
  150,    151,    153,    154,    156,    158,    159,    161,    162,    164,    165,    167,    168,    170,    171,    173,  
  175,    176,    178,    179,    181,    182,    184,    185,    187,    188,    190,    192,    193,    195,    196,    198,  
  199,    201,    202,    204,    205,    207,    209,    210,    212,    213,    215,    216,    218,    219,    221,    222,  
  224,    225,    227,    228,    230,    232,    233,    235,    236,    238,    239,    241,    242,    244,    245,    247,  
  248,    250,    251,    253,    254,    256,    257,    259,    260,    262,    264,    265,    267,    268,    270,    271,  
  273,    274,    276,    277,    279,    280,    282,    283,    285,    286,    288,    289,    291,    292,    294,    295,  
  297,    298,    300,    301,    303,    304,    306,    307,    309,    310,    312,    313,    315,    316,    318,    319,  
  321,    322,    324,    325,    327,    328,    330,    331,    333,    334,    336,    337,    339,    340,    342,    343,  
  344,    346,    347,    349,    350,    352,    353,    355,    356,    358,    359,    361,    362,    364,    365,    367,  
  368,    369,    371,    372,    374,    375,    377,    378,    380,    381,    383,    384,    386,    387,    388,    390,  
  391,    393,    394,    396,    397,    399,    400,    402,    403,    404,    406,    407,    409,    410,    412,    413,  
  414,    416,    417,    419,    420,    422,    423,    424,    426,    427,    429,    430,    432,    433,    434,    436,  
  437,    439,    440,    442,    443,    444,    446,    447,    449,    450,    451,    453,    454,    456,    457,    458,  
  460,    461,    463,    464,    466,    467,    468,    470,    471,    472,    474,    475,    477,    478,    479,    481,  
  482,    484,    485,    486,    488,    489,    491,    492,    493,    495,    496,    497,    499,    500,    501,    503,  
  504,    506,    507,    508,    510,    511,    512,    514,    515,    516,    518,    519,    521,    522,    523,    525,  
  526,    527,    529,    530,    531,    533,    534,    535,    537,    538,    539,    541,    542,    543,    545,    546,  
  547,    549,    550,    551,    553,    554,    555,    557,    558,    559,    561,    562,    563,    564,    566,    567,  
  568,    570,    571,    572,    574,    575,    576,    578,    579,    580,    581,    583,    584,    585,    587,    588,  
  589,    590,    592,    593,    594,    596,    597,    598,    599,    601,    602,    603,    604,    606,    607,    608,  
  609,    611,    612,    613,    615,    616,    617,    618,    620,    621,    622,    623,    625,    626,    627,    628,  
  629,    631,    632,    633,    634,    636,    637,    638,    639,    641,    642,    643,    644,    645,    647,    648,  
  649,    650,    652,    653,    654,    655,    656,    658,    659,    660,    661,    662,    664,    665,    666,    667,  
  668,    670,    671,    672,    673,    674,    675,    677,    678,    679,    680,    681,    683,    684,    685,    686,  
  687,    688,    690,    691,    692,    693,    694,    695,    696,    698,    699,    700,    701,    702,    703,    704,  
  706,    707,    708,    709,    710,    711,    712,    714,    715,    716,    717,    718,    719,    720,    721,    722,  
  724,    725,    726,    727,    728,    729,    730,    731,    732,    734,    735,    736,    737,    738,    739,    740,  
  741,    742,    743,    744,    745,    747,    748,    749,    750,    751,    752,    753,    754,    755,    756,    757,  
  758,    759,    760,    761,    762,    763,    765,    766,    767,    768,    769,    770,    771,    772,    773,    774,  
  775,    776,    777,    778,    779,    780,    781,    782,    783,    784,    785,    786,    787,    788,    789,    790,  
  791,    792,    793,    794,    795,    796,    797,    798,    799,    800,    801,    802,    803,    804,    805,    806,  
  807,    808,    809,    810,    811,    812,    813,    813,    814,    815,    816,    817,    818,    819,    820,    821,  
  822,    823,    824,    825,    826,    827,    828,    828,    829,    830,    831,    832,    833,    834,    835,    836,  
  837,    838,    839,    839,    840,    841,    842,    843,    844,    845,    846,    847,    847,    848,    849,    850,  
  851,    852,    853,    854,    854,    855,    856,    857,    858,    859,    860,    860,    861,    862,    863,    864,  
  865,    865,    866,    867,    868,    869,    870,    870,    871,    872,    873,    874,    875,    875,    876,    877,  
  878,    879,    879,    880,    881,    882,    883,    883,    884,    885,    886,    887,    887,    888,    889,    890,  
  890,    891,    892,    893,    894,    894,    895,    896,    897,    897,    898,    899,    900,    900,    901,    902,  
  903,    903,    904,    905,    906,    906,    907,    908,    908,    909,    910,    911,    911,    912,    913,    913,  
  914,    915,    916,    916,    917,    918,    918,    919,    920,    920,    921,    922,    922,    923,    924,    925,  
  925,    926,    927,    927,    928,    929,    929,    930,    930,    931,    932,    932,    933,    934,    934,    935,  
  936,    936,    937,    938,    938,    939,    939,    940,    941,    941,    942,    943,    943,    944,    944,    945,  
  946,    946,    947,    947,    948,    949,    949,    950,    950,    951,    951,    952,    953,    953,    954,    954,  
  955,    955,    956,    957,    957,    958,    958,    959,    959,    960,    960,    961,    962,    962,    963,    963,  
  964,    964,    965,    965,    966,    966,    967,    967,    968,    968,    969,    969,    970,    970,    971,    971,  
  972,    972,    973,    973,    974,    974,    975,    975,    976,    976,    977,    977,    978,    978,    978,    979,  
  979,    980,    980,    981,    981,    982,    982,    983,    983,    983,    984,    984,    985,    985,    986,    986,  
  986,    987,    987,    988,    988,    988,    989,    989,    990,    990,    990,    991,    991,    992,    992,    992,  
  993,    993,    994,    994,    994,    995,    995,    995,    996,    996,    997,    997,    997,    998,    998,    998,  
  999,    999,    999,   1000,   1000,   1000,   1001,   1001,   1001,   1002,   1002,   1002,   1003,   1003,   1003,   1004,  
 1004,   1004,   1004,   1005,   1005,   1005,   1006,   1006,   1006,   1006,   1007,   1007,   1007,   1008,   1008,   1008,  
 1008,   1009,   1009,   1009,   1009,   1010,   1010,   1010,   1010,   1011,   1011,   1011,   1011,   1012,   1012,   1012,  
 1012,   1013,   1013,   1013,   1013,   1014,   1014,   1014,   1014,   1014,   1015,   1015,   1015,   1015,   1015,   1016,  
 1016,   1016,   1016,   1016,   1017,   1017,   1017,   1017,   1017,   1017,   1018,   1018,   1018,   1018,   1018,   1018,  
 1019,   1019,   1019,   1019,   1019,   1019,   1019,   1020,   1020,   1020,   1020,   1020,   1020,   1020,   1020,   1021,  
 1021,   1021,   1021,   1021,   1021,   1021,   1021,   1021,   1022,   1022,   1022,   1022,   1022,   1022,   1022,   1022,  
 1022,   1022,   1022,   1022,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,  
 1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,  
 1024,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,  
 1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1023,   1022,   1022,   1022,  
 1022,   1022,   1022,   1022,   1022,   1022,   1022,   1022,   1022,   1021,   1021,   1021,   1021,   1021,   1021,   1021,  
 1021,   1021,   1020,   1020,   1020,   1020,   1020,   1020,   1020,   1020,   1019,   1019,   1019,   1019,   1019,   1019,  
 1019,   1018,   1018,   1018,   1018,   1018,   1018,   1017,   1017,   1017,   1017,   1017,   1017,   1016,   1016,   1016,  
 1016,   1016,   1015,   1015,   1015,   1015,   1015,   1014,   1014,   1014,   1014,   1014,   1013,   1013,   1013,   1013,  
 1012,   1012,   1012,   1012,   1011,   1011,   1011,   1011,   1010,   1010,   1010,   1010,   1009,   1009,   1009,   1009,  
 1008,   1008,   1008,   1008,   1007,   1007,   1007,   1006,   1006,   1006,   1006,   1005,   1005,   1005,   1004,   1004,  
 1004,   1004,   1003,   1003,   1003,   1002,   1002,   1002,   1001,   1001,   1001,   1000,   1000,   1000,    999,    999,  
  999,    998,    998,    998,    997,    997,    997,    996,    996,    995,    995,    995,    994,    994,    994,    993,  
  993,    992,    992,    992,    991,    991,    990,    990,    990,    989,    989,    988,    988,    988,    987,    987,  
  986,    986,    986,    985,    985,    984,    984,    983,    983,    983,    982,    982,    981,    981,    980,    980,  
  979,    979,    978,    978,    978,    977,    977,    976,    976,    975,    975,    974,    974,    973,    973,    972,  
  972,    971,    971,    970,    970,    969,    969,    968,    968,    967,    967,    966,    966,    965,    965,    964,  
  964,    963,    963,    962,    962,    961,    960,    960,    959,    959,    958,    958,    957,    957,    956,    955,  
  955,    954,    954,    953,    953,    952,    951,    951,    950,    950,    949,    949,    948,    947,    947,    946,  
  946,    945,    944,    944,    943,    943,    942,    941,    941,    940,    939,    939,    938,    938,    937,    936,  
  936,    935,    934,    934,    933,    932,    932,    931,    930,    930,    929,    929,    928,    927,    927,    926,  
  925,    925,    924,    923,    922,    922,    921,    920,    920,    919,    918,    918,    917,    916,    916,    915,  
  914,    913,    913,    912,    911,    911,    910,    909,    908,    908,    907,    906,    906,    905,    904,    903,  
  903,    902,    901,    900,    900,    899,    898,    897,    897,    896,    895,    894,    894,    893,    892,    891,  
  890,    890,    889,    888,    887,    887,    886,    885,    884,    883,    883,    882,    881,    880,    879,    879,  
  878,    877,    876,    875,    875,    874,    873,    872,    871,    870,    870,    869,    868,    867,    866,    865,  
  865,    864,    863,    862,    861,    860,    860,    859,    858,    857,    856,    855,    854,    854,    853,    852,  
  851,    850,    849,    848,    847,    847,    846,    845,    844,    843,    842,    841,    840,    839,    839,    838,  
  837,    836,    835,    834,    833,    832,    831,    830,    829,    828,    828,    827,    826,    825,    824,    823,  
  822,    821,    820,    819,    818,    817,    816,    815,    814,    813,    813,    812,    811,    810,    809,    808,  
  807,    806,    805,    804,    803,    802,    801,    800,    799,    798,    797,    796,    795,    794,    793,    792,  
  791,    790,    789,    788,    787,    786,    785,    784,    783,    782,    781,    780,    779,    778,    777,    776,  
  775,    774,    773,    772,    771,    770,    769,    768,    767,    766,    765,    763,    762,    761,    760,    759,  
  758,    757,    756,    755,    754,    753,    752,    751,    750,    749,    748,    747,    745,    744,    743,    742,  
  741,    740,    739,    738,    737,    736,    735,    734,    732,    731,    730,    729,    728,    727,    726,    725,  
  724,    722,    721,    720,    719,    718,    717,    716,    715,    714,    712,    711,    710,    709,    708,    707,  
  706,    704,    703,    702,    701,    700,    699,    698,    696,    695,    694,    693,    692,    691,    690,    688,  
  687,    686,    685,    684,    683,    681,    680,    679,    678,    677,    675,    674,    673,    672,    671,    670,  
  668,    667,    666,    665,    664,    662,    661,    660,    659,    658,    656,    655,    654,    653,    652,    650,  
  649,    648,    647,    645,    644,    643,    642,    641,    639,    638,    637,    636,    634,    633,    632,    631,  
  629,    628,    627,    626,    625,    623,    622,    621,    620,    618,    617,    616,    615,    613,    612,    611,  
  609,    608,    607,    606,    604,    603,    602,    601,    599,    598,    597,    596,    594,    593,    592,    590,  
  589,    588,    587,    585,    584,    583,    581,    580,    579,    578,    576,    575,    574,    572,    571,    570,  
  568,    567,    566,    564,    563,    562,    561,    559,    558,    557,    555,    554,    553,    551,    550,    549,  
  547,    546,    545,    543,    542,    541,    539,    538,    537,    535,    534,    533,    531,    530,    529,    527,  
  526,    525,    523,    522,    521,    519,    518,    516,    515,    514,    512,    511,    510,    508,    507,    506,  
  504,    503,    501,    500,    499,    497,    496,    495,    493,    492,    491,    489,    488,    486,    485,    484,  
  482,    481,    479,    478,    477,    475,    474,    472,    471,    470,    468,    467,    466,    464,    463,    461,  
  460,    458,    457,    456,    454,    453,    451,    450,    449,    447,    446,    444,    443,    442,    440,    439,  
  437,    436,    434,    433,    432,    430,    429,    427,    426,    424,    423,    422,    420,    419,    417,    416,  
  414,    413,    412,    410,    409,    407,    406,    404,    403,    402,    400,    399,    397,    396,    394,    393,  
  391,    390,    388,    387,    386,    384,    383,    381,    380,    378,    377,    375,    374,    372,    371,    369,  
  368,    367,    365,    364,    362,    361,    359,    358,    356,    355,    353,    352,    350,    349,    347,    346,  
  344,    343,    342,    340,    339,    337,    336,    334,    333,    331,    330,    328,    327,    325,    324,    322,  
  321,    319,    318,    316,    315,    313,    312,    310,    309,    307,    306,    304,    303,    301,    300,    298,  
  297,    295,    294,    292,    291,    289,    288,    286,    285,    283,    282,    280,    279,    277,    276,    274,  
  273,    271,    270,    268,    267,    265,    264,    262,    260,    259,    257,    256,    254,    253,    251,    250,  
  248,    247,    245,    244,    242,    241,    239,    238,    236,    235,    233,    232,    230,    228,    227,    225,  
  224,    222,    221,    219,    218,    216,    215,    213,    212,    210,    209,    207,    205,    204,    202,    201,  
  199,    198,    196,    195,    193,    192,    190,    188,    187,    185,    184,    182,    181,    179,    178,    176,  
  175,    173,    171,    170,    168,    167,    165,    164,    162,    161,    159,    158,    156,    154,    153,    151,  
  150,    148,    147,    145,    144,    142,    140,    139,    137,    136,    134,    133,    131,    130,    128,    126,  
  125,    123,    122,    120,    119,    117,    115,    114,    112,    111,    109,    108,    106,    105,    103,    101,  
  100,     98,     97,     95,     94,     92,     90,     89,     87,     86,     84,     83,     81,     80,     78,     76,  
   75,     73,     72,     70,     69,     67,     65,     64,     62,     61,     59,     58,     56,     54,     53,     51,  
   50,     48,     47,     45,     43,     42,     40,     39,     37,     36,     34,     32,     31,     29,     28,     26,  
   25,     23,     21,     20,     18,     17,     15,     14,     12,     10,      9,      7,      6,      4,      3,      1,  
    0,     -1,     -3,     -4,     -6,     -7,     -9,    -10,    -12,    -14,    -15,    -17,    -18,    -20,    -21,    -23,  
  -25,    -26,    -28,    -29,    -31,    -32,    -34,    -36,    -37,    -39,    -40,    -42,    -43,    -45,    -47,    -48,  
  -50,    -51,    -53,    -54,    -56,    -58,    -59,    -61,    -62,    -64,    -65,    -67,    -69,    -70,    -72,    -73,  
  -75,    -76,    -78,    -80,    -81,    -83,    -84,    -86,    -87,    -89,    -90,    -92,    -94,    -95,    -97,    -98,  
 -100,   -101,   -103,   -105,   -106,   -108,   -109,   -111,   -112,   -114,   -115,   -117,   -119,   -120,   -122,   -123,  
 -125,   -126,   -128,   -130,   -131,   -133,   -134,   -136,   -137,   -139,   -140,   -142,   -144,   -145,   -147,   -148,  
 -150,   -151,   -153,   -154,   -156,   -158,   -159,   -161,   -162,   -164,   -165,   -167,   -168,   -170,   -171,   -173,  
 -175,   -176,   -178,   -179,   -181,   -182,   -184,   -185,   -187,   -188,   -190,   -192,   -193,   -195,   -196,   -198,  
 -199,   -201,   -202,   -204,   -205,   -207,   -209,   -210,   -212,   -213,   -215,   -216,   -218,   -219,   -221,   -222,  
 -224,   -225,   -227,   -228,   -230,   -232,   -233,   -235,   -236,   -238,   -239,   -241,   -242,   -244,   -245,   -247,  
 -248,   -250,   -251,   -253,   -254,   -256,   -257,   -259,   -260,   -262,   -264,   -265,   -267,   -268,   -270,   -271,  
 -273,   -274,   -276,   -277,   -279,   -280,   -282,   -283,   -285,   -286,   -288,   -289,   -291,   -292,   -294,   -295,  
 -297,   -298,   -300,   -301,   -303,   -304,   -306,   -307,   -309,   -310,   -312,   -313,   -315,   -316,   -318,   -319,  
 -321,   -322,   -324,   -325,   -327,   -328,   -330,   -331,   -333,   -334,   -336,   -337,   -339,   -340,   -342,   -343,  
 -344,   -346,   -347,   -349,   -350,   -352,   -353,   -355,   -356,   -358,   -359,   -361,   -362,   -364,   -365,   -367,  
 -368,   -369,   -371,   -372,   -374,   -375,   -377,   -378,   -380,   -381,   -383,   -384,   -386,   -387,   -388,   -390,  
 -391,   -393,   -394,   -396,   -397,   -399,   -400,   -402,   -403,   -404,   -406,   -407,   -409,   -410,   -412,   -413,  
 -414,   -416,   -417,   -419,   -420,   -422,   -423,   -424,   -426,   -427,   -429,   -430,   -432,   -433,   -434,   -436,  
 -437,   -439,   -440,   -442,   -443,   -444,   -446,   -447,   -449,   -450,   -451,   -453,   -454,   -456,   -457,   -458,  
 -460,   -461,   -463,   -464,   -466,   -467,   -468,   -470,   -471,   -472,   -474,   -475,   -477,   -478,   -479,   -481,  
 -482,   -484,   -485,   -486,   -488,   -489,   -491,   -492,   -493,   -495,   -496,   -497,   -499,   -500,   -501,   -503,  
 -504,   -506,   -507,   -508,   -510,   -511,   -512,   -514,   -515,   -516,   -518,   -519,   -521,   -522,   -523,   -525,  
 -526,   -527,   -529,   -530,   -531,   -533,   -534,   -535,   -537,   -538,   -539,   -541,   -542,   -543,   -545,   -546,  
 -547,   -549,   -550,   -551,   -553,   -554,   -555,   -557,   -558,   -559,   -561,   -562,   -563,   -564,   -566,   -567,  
 -568,   -570,   -571,   -572,   -574,   -575,   -576,   -578,   -579,   -580,   -581,   -583,   -584,   -585,   -587,   -588,  
 -589,   -590,   -592,   -593,   -594,   -596,   -597,   -598,   -599,   -601,   -602,   -603,   -604,   -606,   -607,   -608,  
 -609,   -611,   -612,   -613,   -615,   -616,   -617,   -618,   -620,   -621,   -622,   -623,   -625,   -626,   -627,   -628,  
 -629,   -631,   -632,   -633,   -634,   -636,   -637,   -638,   -639,   -641,   -642,   -643,   -644,   -645,   -647,   -648,  
 -649,   -650,   -652,   -653,   -654,   -655,   -656,   -658,   -659,   -660,   -661,   -662,   -664,   -665,   -666,   -667,  
 -668,   -670,   -671,   -672,   -673,   -674,   -675,   -677,   -678,   -679,   -680,   -681,   -683,   -684,   -685,   -686,  
 -687,   -688,   -690,   -691,   -692,   -693,   -694,   -695,   -696,   -698,   -699,   -700,   -701,   -702,   -703,   -704,  
 -706,   -707,   -708,   -709,   -710,   -711,   -712,   -714,   -715,   -716,   -717,   -718,   -719,   -720,   -721,   -722,  
 -724,   -725,   -726,   -727,   -728,   -729,   -730,   -731,   -732,   -734,   -735,   -736,   -737,   -738,   -739,   -740,  
 -741,   -742,   -743,   -744,   -745,   -747,   -748,   -749,   -750,   -751,   -752,   -753,   -754,   -755,   -756,   -757,  
 -758,   -759,   -760,   -761,   -762,   -763,   -765,   -766,   -767,   -768,   -769,   -770,   -771,   -772,   -773,   -774,  
 -775,   -776,   -777,   -778,   -779,   -780,   -781,   -782,   -783,   -784,   -785,   -786,   -787,   -788,   -789,   -790,  
 -791,   -792,   -793,   -794,   -795,   -796,   -797,   -798,   -799,   -800,   -801,   -802,   -803,   -804,   -805,   -806,  
 -807,   -808,   -809,   -810,   -811,   -812,   -813,   -813,   -814,   -815,   -816,   -817,   -818,   -819,   -820,   -821,  
 -822,   -823,   -824,   -825,   -826,   -827,   -828,   -828,   -829,   -830,   -831,   -832,   -833,   -834,   -835,   -836,  
 -837,   -838,   -839,   -839,   -840,   -841,   -842,   -843,   -844,   -845,   -846,   -847,   -847,   -848,   -849,   -850,  
 -851,   -852,   -853,   -854,   -854,   -855,   -856,   -857,   -858,   -859,   -860,   -860,   -861,   -862,   -863,   -864,  
 -865,   -865,   -866,   -867,   -868,   -869,   -870,   -870,   -871,   -872,   -873,   -874,   -875,   -875,   -876,   -877,  
 -878,   -879,   -879,   -880,   -881,   -882,   -883,   -883,   -884,   -885,   -886,   -887,   -887,   -888,   -889,   -890,  
 -890,   -891,   -892,   -893,   -894,   -894,   -895,   -896,   -897,   -897,   -898,   -899,   -900,   -900,   -901,   -902,  
 -903,   -903,   -904,   -905,   -906,   -906,   -907,   -908,   -908,   -909,   -910,   -911,   -911,   -912,   -913,   -913,  
 -914,   -915,   -916,   -916,   -917,   -918,   -918,   -919,   -920,   -920,   -921,   -922,   -922,   -923,   -924,   -925,  
 -925,   -926,   -927,   -927,   -928,   -929,   -929,   -930,   -930,   -931,   -932,   -932,   -933,   -934,   -934,   -935,  
 -936,   -936,   -937,   -938,   -938,   -939,   -939,   -940,   -941,   -941,   -942,   -943,   -943,   -944,   -944,   -945,  
 -946,   -946,   -947,   -947,   -948,   -949,   -949,   -950,   -950,   -951,   -951,   -952,   -953,   -953,   -954,   -954,  
 -955,   -955,   -956,   -957,   -957,   -958,   -958,   -959,   -959,   -960,   -960,   -961,   -962,   -962,   -963,   -963,  
 -964,   -964,   -965,   -965,   -966,   -966,   -967,   -967,   -968,   -968,   -969,   -969,   -970,   -970,   -971,   -971,  
 -972,   -972,   -973,   -973,   -974,   -974,   -975,   -975,   -976,   -976,   -977,   -977,   -978,   -978,   -978,   -979,  
 -979,   -980,   -980,   -981,   -981,   -982,   -982,   -983,   -983,   -983,   -984,   -984,   -985,   -985,   -986,   -986,  
 -986,   -987,   -987,   -988,   -988,   -988,   -989,   -989,   -990,   -990,   -990,   -991,   -991,   -992,   -992,   -992,  
 -993,   -993,   -994,   -994,   -994,   -995,   -995,   -995,   -996,   -996,   -997,   -997,   -997,   -998,   -998,   -998,  
 -999,   -999,   -999,  -1000,  -1000,  -1000,  -1001,  -1001,  -1001,  -1002,  -1002,  -1002,  -1003,  -1003,  -1003,  -1004,  
-1004,  -1004,  -1004,  -1005,  -1005,  -1005,  -1006,  -1006,  -1006,  -1006,  -1007,  -1007,  -1007,  -1008,  -1008,  -1008,  
-1008,  -1009,  -1009,  -1009,  -1009,  -1010,  -1010,  -1010,  -1010,  -1011,  -1011,  -1011,  -1011,  -1012,  -1012,  -1012,  
-1012,  -1013,  -1013,  -1013,  -1013,  -1014,  -1014,  -1014,  -1014,  -1014,  -1015,  -1015,  -1015,  -1015,  -1015,  -1016,  
-1016,  -1016,  -1016,  -1016,  -1017,  -1017,  -1017,  -1017,  -1017,  -1017,  -1018,  -1018,  -1018,  -1018,  -1018,  -1018,  
-1019,  -1019,  -1019,  -1019,  -1019,  -1019,  -1019,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1021,  
-1021,  -1021,  -1021,  -1021,  -1021,  -1021,  -1021,  -1021,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  
-1022,  -1022,  -1022,  -1022,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  
-1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  
-1024,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  
-1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1023,  -1022,  -1022,  -1022,  
-1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1022,  -1021,  -1021,  -1021,  -1021,  -1021,  -1021,  -1021,  
-1021,  -1021,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1020,  -1019,  -1019,  -1019,  -1019,  -1019,  -1019,  
-1019,  -1018,  -1018,  -1018,  -1018,  -1018,  -1018,  -1017,  -1017,  -1017,  -1017,  -1017,  -1017,  -1016,  -1016,  -1016,  
-1016,  -1016,  -1015,  -1015,  -1015,  -1015,  -1015,  -1014,  -1014,  -1014,  -1014,  -1014,  -1013,  -1013,  -1013,  -1013,  
-1012,  -1012,  -1012,  -1012,  -1011,  -1011,  -1011,  -1011,  -1010,  -1010,  -1010,  -1010,  -1009,  -1009,  -1009,  -1009,  
-1008,  -1008,  -1008,  -1008,  -1007,  -1007,  -1007,  -1006,  -1006,  -1006,  -1006,  -1005,  -1005,  -1005,  -1004,  -1004,  
-1004,  -1004,  -1003,  -1003,  -1003,  -1002,  -1002,  -1002,  -1001,  -1001,  -1001,  -1000,  -1000,  -1000,   -999,   -999,  
 -999,   -998,   -998,   -998,   -997,   -997,   -997,   -996,   -996,   -995,   -995,   -995,   -994,   -994,   -994,   -993,  
 -993,   -992,   -992,   -992,   -991,   -991,   -990,   -990,   -990,   -989,   -989,   -988,   -988,   -988,   -987,   -987,  
 -986,   -986,   -986,   -985,   -985,   -984,   -984,   -983,   -983,   -983,   -982,   -982,   -981,   -981,   -980,   -980,  
 -979,   -979,   -978,   -978,   -978,   -977,   -977,   -976,   -976,   -975,   -975,   -974,   -974,   -973,   -973,   -972,  
 -972,   -971,   -971,   -970,   -970,   -969,   -969,   -968,   -968,   -967,   -967,   -966,   -966,   -965,   -965,   -964,  
 -964,   -963,   -963,   -962,   -962,   -961,   -960,   -960,   -959,   -959,   -958,   -958,   -957,   -957,   -956,   -955,  
 -955,   -954,   -954,   -953,   -953,   -952,   -951,   -951,   -950,   -950,   -949,   -949,   -948,   -947,   -947,   -946,  
 -946,   -945,   -944,   -944,   -943,   -943,   -942,   -941,   -941,   -940,   -939,   -939,   -938,   -938,   -937,   -936,  
 -936,   -935,   -934,   -934,   -933,   -932,   -932,   -931,   -930,   -930,   -929,   -929,   -928,   -927,   -927,   -926,  
 -925,   -925,   -924,   -923,   -922,   -922,   -921,   -920,   -920,   -919,   -918,   -918,   -917,   -916,   -916,   -915,  
 -914,   -913,   -913,   -912,   -911,   -911,   -910,   -909,   -908,   -908,   -907,   -906,   -906,   -905,   -904,   -903,  
 -903,   -902,   -901,   -900,   -900,   -899,   -898,   -897,   -897,   -896,   -895,   -894,   -894,   -893,   -892,   -891,  
 -890,   -890,   -889,   -888,   -887,   -887,   -886,   -885,   -884,   -883,   -883,   -882,   -881,   -880,   -879,   -879,  
 -878,   -877,   -876,   -875,   -875,   -874,   -873,   -872,   -871,   -870,   -870,   -869,   -868,   -867,   -866,   -865,  
 -865,   -864,   -863,   -862,   -861,   -860,   -860,   -859,   -858,   -857,   -856,   -855,   -854,   -854,   -853,   -852,  
 -851,   -850,   -849,   -848,   -847,   -847,   -846,   -845,   -844,   -843,   -842,   -841,   -840,   -839,   -839,   -838,  
 -837,   -836,   -835,   -834,   -833,   -832,   -831,   -830,   -829,   -828,   -828,   -827,   -826,   -825,   -824,   -823,  
 -822,   -821,   -820,   -819,   -818,   -817,   -816,   -815,   -814,   -813,   -813,   -812,   -811,   -810,   -809,   -808,  
 -807,   -806,   -805,   -804,   -803,   -802,   -801,   -800,   -799,   -798,   -797,   -796,   -795,   -794,   -793,   -792,  
 -791,   -790,   -789,   -788,   -787,   -786,   -785,   -784,   -783,   -782,   -781,   -780,   -779,   -778,   -777,   -776,  
 -775,   -774,   -773,   -772,   -771,   -770,   -769,   -768,   -767,   -766,   -765,   -763,   -762,   -761,   -760,   -759,  
 -758,   -757,   -756,   -755,   -754,   -753,   -752,   -751,   -750,   -749,   -748,   -747,   -745,   -744,   -743,   -742,  
 -741,   -740,   -739,   -738,   -737,   -736,   -735,   -734,   -732,   -731,   -730,   -729,   -728,   -727,   -726,   -725,  
 -724,   -722,   -721,   -720,   -719,   -718,   -717,   -716,   -715,   -714,   -712,   -711,   -710,   -709,   -708,   -707,  
 -706,   -704,   -703,   -702,   -701,   -700,   -699,   -698,   -696,   -695,   -694,   -693,   -692,   -691,   -690,   -688,  
 -687,   -686,   -685,   -684,   -683,   -681,   -680,   -679,   -678,   -677,   -675,   -674,   -673,   -672,   -671,   -670,  
 -668,   -667,   -666,   -665,   -664,   -662,   -661,   -660,   -659,   -658,   -656,   -655,   -654,   -653,   -652,   -650,  
 -649,   -648,   -647,   -645,   -644,   -643,   -642,   -641,   -639,   -638,   -637,   -636,   -634,   -633,   -632,   -631,  
 -629,   -628,   -627,   -626,   -625,   -623,   -622,   -621,   -620,   -618,   -617,   -616,   -615,   -613,   -612,   -611,  
 -609,   -608,   -607,   -606,   -604,   -603,   -602,   -601,   -599,   -598,   -597,   -596,   -594,   -593,   -592,   -590,  
 -589,   -588,   -587,   -585,   -584,   -583,   -581,   -580,   -579,   -578,   -576,   -575,   -574,   -572,   -571,   -570,  
 -568,   -567,   -566,   -564,   -563,   -562,   -561,   -559,   -558,   -557,   -555,   -554,   -553,   -551,   -550,   -549,  
 -547,   -546,   -545,   -543,   -542,   -541,   -539,   -538,   -537,   -535,   -534,   -533,   -531,   -530,   -529,   -527,  
 -526,   -525,   -523,   -522,   -521,   -519,   -518,   -516,   -515,   -514,   -512,   -511,   -510,   -508,   -507,   -506,  
 -504,   -503,   -501,   -500,   -499,   -497,   -496,   -495,   -493,   -492,   -491,   -489,   -488,   -486,   -485,   -484,  
 -482,   -481,   -479,   -478,   -477,   -475,   -474,   -472,   -471,   -470,   -468,   -467,   -466,   -464,   -463,   -461,  
 -460,   -458,   -457,   -456,   -454,   -453,   -451,   -450,   -449,   -447,   -446,   -444,   -443,   -442,   -440,   -439,  
 -437,   -436,   -434,   -433,   -432,   -430,   -429,   -427,   -426,   -424,   -423,   -422,   -420,   -419,   -417,   -416,  
 -414,   -413,   -412,   -410,   -409,   -407,   -406,   -404,   -403,   -402,   -400,   -399,   -397,   -396,   -394,   -393,  
 -391,   -390,   -388,   -387,   -386,   -384,   -383,   -381,   -380,   -378,   -377,   -375,   -374,   -372,   -371,   -369,  
 -368,   -367,   -365,   -364,   -362,   -361,   -359,   -358,   -356,   -355,   -353,   -352,   -350,   -349,   -347,   -346,  
 -344,   -343,   -342,   -340,   -339,   -337,   -336,   -334,   -333,   -331,   -330,   -328,   -327,   -325,   -324,   -322,  
 -321,   -319,   -318,   -316,   -315,   -313,   -312,   -310,   -309,   -307,   -306,   -304,   -303,   -301,   -300,   -298,  
 -297,   -295,   -294,   -292,   -291,   -289,   -288,   -286,   -285,   -283,   -282,   -280,   -279,   -277,   -276,   -274,  
 -273,   -271,   -270,   -268,   -267,   -265,   -264,   -262,   -260,   -259,   -257,   -256,   -254,   -253,   -251,   -250,  
 -248,   -247,   -245,   -244,   -242,   -241,   -239,   -238,   -236,   -235,   -233,   -232,   -230,   -228,   -227,   -225,  
 -224,   -222,   -221,   -219,   -218,   -216,   -215,   -213,   -212,   -210,   -209,   -207,   -205,   -204,   -202,   -201,  
 -199,   -198,   -196,   -195,   -193,   -192,   -190,   -188,   -187,   -185,   -184,   -182,   -181,   -179,   -178,   -176,  
 -175,   -173,   -171,   -170,   -168,   -167,   -165,   -164,   -162,   -161,   -159,   -158,   -156,   -154,   -153,   -151,  
 -150,   -148,   -147,   -145,   -144,   -142,   -140,   -139,   -137,   -136,   -134,   -133,   -131,   -130,   -128,   -126,  
 -125,   -123,   -122,   -120,   -119,   -117,   -115,   -114,   -112,   -111,   -109,   -108,   -106,   -105,   -103,   -101,  
 -100,    -98,    -97,    -95,    -94,    -92,    -90,    -89,    -87,    -86,    -84,    -83,    -81,    -80,    -78,    -76,  
  -75,    -73,    -72,    -70,    -69,    -67,    -65,    -64,    -62,    -61,    -59,    -58,    -56,    -54,    -53,    -51,  
  -50,    -48,    -47,    -45,    -43,    -42,    -40,    -39,    -37,    -36,    -34,    -32,    -31,    -29,    -28,    -26,  
  -25,    -23,    -21,    -20,    -18,    -17,    -15,    -14,    -12,    -10,     -9,     -7,     -6,     -4,     -3,     -1,  
    0,
};
#define STM32_FLASH_BASE        0x8000000           //SOC Flash Base Address 
#define Data_Store_Arrdess      0x8007C00           //Flash data storage Address 
#define MENU_NUM                7                   //Number of submenus 
static void LL_Init(void);
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_SPI1_Init(void);
static void MX_TIM1_Init(void);
static void MX_TIM3_Init(void);
static void MX_TIM6_Init(void);
static void MX_IWDG_Init(void);


void SetModeCheck(void);
void UsDelay(uint16_t us);
void Output(int32_t theta,uint8_t effort);
int16_t Mod(int32_t xMod,int16_t mMod);
void OneStep(void);
void FlashUnlock(void);
void FlashLock(void);
uint8_t FlashGetStatus(void);
uint8_t FlashWaitDone(uint16_t time);
uint8_t FlashErasePage(uint32_t paddr);
void FlashErase32K(void);
uint8_t FlashWriteHalfWord(uint32_t faddr,uint16_t dat);
uint16_t FlashReadHalfWord(uint32_t faddr);
int fputc(int c,FILE *stream); 
int fgetc(FILE *stream); 
void SerialCheck(void);
uint16_t ReadValue(uint16_t RegValue);
void WriteValue(uint16_t RegAdd,uint16_t RegValue);
uint16_t ReadState(void);
uint16_t ReadAngle(void);
void CalibrateEncoder(void);
void KeyScan(void);
void Prompt_show(void);
void restart_init(void);

int16_t kp=30;                //
int16_t ki=10;                //
int16_t kd=250;               //


const uint8_t LPFA=125; 
const uint8_t LPFB=3;

int32_t s=0;//
int32_t s_1=0;
int32_t s_sum=0;//
int32_t r=0;
int32_t r_1=0;
uint8_t dir=1;//
int16_t y=0;//
int16_t y_1=0;
int32_t yw=0;//
int32_t yw_1=0;
int16_t advance=0;//
int32_t wrap_count=0;//
int32_t e=0;//
int32_t iterm=0;//
int32_t dterm=0;//
int16_t u=0;//
int32_t stepnumber=0;//
uint8_t stepangle=0;//

uint16_t hccount=0;//
uint8_t closemode;//
uint8_t enmode=0;//

uint8_t KEY_Select_flag=0;       //
uint8_t KEY_Back_flag=0;         //
uint8_t KEY_Confirm_flag=0;      //

uint8_t Enter_exit_flag =0;      //
uint8_t Calibration_flag=0;      //
uint8_t Menu_Num =0;             //
uint8_t Menu_Num_item=0;         //
uint8_t Menu_move_num=0;         //
volatile uint8_t Menu_update_flag=1;      //
volatile uint8_t Data_update_flag =1; //
volatile uint16_t Data_update_Count =0; //25ms

uint8_t CalibrateEncoder_finish_flag=0; //
uint8_t Second_Calibrate_flag=0;        //
uint8_t Second_Menu=0;                  //
uint8_t Menu_Num2_item=0;               //  
uint8_t Menu_Num3_item=0;               //  
uint8_t Menu_Num4_item=0;               //  
uint8_t Menu_Num5_item=0;               // 
uint8_t Menu_Num6_item=0;               // 
uint8_t enter_num=0; 
int16_t Motor_speed =0;
int16_t wap1=0;
int16_t wap2=0;
int16_t Motor_speed_count=0; 
uint8_t start_measu_V_flag=0;
uint8_t measure_once_flag=0; 

uint8_t Currents=0;                 //
uint8_t Currents_Set=0;
uint8_t Microstep_Set=0;            //
uint8_t Microstep=0;                //
uint8_t Dir_Enable=0x00;           //
                                    //!ENABLE = 
                                    //ENABLE = 
uint8_t Motor_Dir=0;               //

uint8_t Motor_Enable=0;            //
uint8_t Motor_mode=0;              // 
volatile uint8_t Motor_ENmode_flag=0; //
volatile uint8_t enter1_once_flag =1;    //
volatile uint8_t enter2_once_flag =0;

volatile uint8_t dir1_once_flag =1;      //
volatile uint8_t dir2_once_flag =0;
volatile uint8_t close_loop1_flag =1;      //
volatile uint8_t close_loop2_flag =0;

volatile uint8_t flash_store_flag=0;    //
uint16_t table1[14];                    //
volatile uint8_t Reset_status_flag=0;                    

uint8_t Receive_Count=0;                //    
uint8_t Rx1_temp_num=0;                 //
uint8_t Receive_finish_flag=0;          //
uint8_t Rx1_buff[8];
volatile uint8_t  Communications_Process_flag=0;     
volatile uint8_t  Uart_CRC_flag=0;                   //  
volatile uint8_t  frame_Error_flag=0;                //
volatile uint8_t  Urat_CRC_Correct_flag=0;           //
//uint8_t Receive_statu=0x00;
int16_t value_Temp=0;

//
//ReadAddr:
//pBuffer:
//NumToWrite:
void STMFLASH_Read(uint32_t ReadAddr,uint16_t *pBuffer,uint16_t NumToRead)   	
{
	uint16_t i;
	for(i=0;i<NumToRead;i++)
	{
        pBuffer[i]=FlashReadHalfWord(ReadAddr);//
		ReadAddr+=2;//
	}
}
//
//WriteAddr:
//pBuffer:
//NumToWrite:   
void STMFLASH_Write_NoCheck(uint32_t WriteAddr,uint16_t *pBuffer,uint16_t NumToWrite)   
{ 			 		 
	uint16_t i;
	for(i=0;i<NumToWrite;i++)
	{
		FlashWriteHalfWord(WriteAddr,pBuffer[i]);
	    WriteAddr+=2;//
	}  
} 

//WriteAddr:
//pBuffer:
//NumToWrite:
#define STM32_FLASH_SIZE 64
#if STM32_FLASH_SIZE<256
#define STM_SECTOR_SIZE 1024 //
#else 
#define STM_SECTOR_SIZE	2048
#endif		 
uint16_t STMFLASH_BUF[STM_SECTOR_SIZE/2];//
void STMFLASH_Write(uint32_t WriteAddr,uint16_t *pBuffer,uint16_t NumToWrite)	
{
	uint32_t secpos;	   //
	uint16_t secoff;	   //
	uint16_t secremain; //
 	uint16_t i;    
	uint32_t offaddr;   //
	if(WriteAddr<STM32_FLASH_BASE||(WriteAddr>=(STM32_FLASH_BASE+1024*STM32_FLASH_SIZE)))return;
	FlashUnlock();						//
	offaddr=WriteAddr-STM32_FLASH_BASE;		//
	secpos=offaddr/STM_SECTOR_SIZE;			//
	secoff=(offaddr%STM_SECTOR_SIZE)/2;		//
	secremain=STM_SECTOR_SIZE/2-secoff;		//   
	if(NumToWrite<=secremain)secremain=NumToWrite;//
	while(1) 
	{	
		STMFLASH_Read(secpos*STM_SECTOR_SIZE+STM32_FLASH_BASE,STMFLASH_BUF,STM_SECTOR_SIZE/2);//
		for(i=0;i<secremain;i++)//
		{
			if(STMFLASH_BUF[secoff+i]!=0XFFFF)break;//
		}
		if(i<secremain)//
		{
			FlashErasePage(secpos*STM_SECTOR_SIZE+STM32_FLASH_BASE);//
			for(i=0;i<secremain;i++)//
			{
				STMFLASH_BUF[i+secoff]=pBuffer[i];	  
			}
			STMFLASH_Write_NoCheck(secpos*STM_SECTOR_SIZE+STM32_FLASH_BASE,STMFLASH_BUF,STM_SECTOR_SIZE/2);//  
		}else STMFLASH_Write_NoCheck(WriteAddr,pBuffer,secremain);//
		if(NumToWrite==secremain)break;//
		else//
		{
			secpos++;				//
			secoff=0;				// 	 
		   	pBuffer+=secremain;  	
			WriteAddr+=secremain*2;	//   
		   	NumToWrite-=secremain;	//
			if(NumToWrite>(STM_SECTOR_SIZE/2))secremain=STM_SECTOR_SIZE/2;//
			else secremain=NumToWrite;//
		}	 
	};	
	FlashLock();//
}
//OLEDOK
//2019-10-21 
//2019-10-22 
//2019-10-23  
//2019-10-28
//2019-10-29 
//2019-11-02 
//2019-11-04  
//2019-11-07 
//2019-11-11 
//2019-11-15 
//2019-11-18 
//2019-11-19 
//2020-01-03 
//
int main(void)
{
 
//    uint16_t table[10]={0xA,0xAA,0xAAA,0xB,5,6,7,8,9,10};
    LL_Init();
    SystemClock_Config();
    MX_GPIO_Init();                           
    OLED_Init();                                
    OLED_ShowString(0,0,"Oled Init...OK");
    MX_SPI1_Init();
    MX_TIM3_Init();
   
    LL_mDelay(100);

    OLED_Clear();                             
    OLED_ShowString(0,0,"Close Loop Model");

//    STMFLASH_Write(Data_Store_Arrdess,table,10);
//    LL_mDelay(2);
//    STMFLASH_Read(Data_Store_Arrdess,table1,10);
//while(1);
    FlashUnlock();
    Calibration_flag = FlashReadHalfWord(Data_Store_Arrdess);
    FlashLock();
    if(Calibration_flag == 0xAA){
        OLED_ShowString(0,25,"  Calibration  ");
        OLED_ShowString(40,45,"  OK!");
        LL_mDelay(500);
        
        OLED_Clear();   
        OLED_ShowString(0,2,"Simp:  0000 RPM");
        OLED_ShowString(0,22," Err:  000.00 ");
        OLED_ShowString(0,42," Deg:  0000.0");
        Menu_Num_item=2;                                        
        Second_Menu=1;                                          
        Menu_update_flag=0;                                    
        Menu_Num=1;                                               
        Reset_status_flag=1;                                    
//      Menu_Num2_item=8;                                       
//      Motor_Enable = enmode;                                
/*****************************************************/

        STMFLASH_Read(Data_Store_Arrdess,table1,14);            
        SetModeCheck();                 
        
        Currents=table1[1];             
        Menu_Num2_item =table1[2];
        
        stepangle=table1[3];             
        Menu_Num3_item=table1[4];        
        
        Motor_ENmode_flag =table1[5];    
        Menu_Num4_item= table1[6];  
        
        Motor_Dir =table1[7];            
        Menu_Num5_item =table1[8];
        
//        Motor_Dir =table1[9];           
//        Menu_Num5_item =table1[10];

        kp = table1[11];                //Kp
        ki = table1[12];                //Ki
        kd = table1[13];                //Kd
    }
    else{
        OLED_ShowString(48,16,"NOT");
        OLED_ShowString(16,32,"Calibration");
        OLED_ShowString(0,48,"Please calibrate");
        LL_mDelay(500);
        //
        OLED_Clear();
        OLED_ShowString(0,0,"->");

        while(1){//
            KeyScan();
            Oled_display();
        }            
    }
  
    NVIC_EnableIRQ(EXTI0_1_IRQn);                               //
    NVIC_EnableIRQ(EXTI2_3_IRQn);                               //
    MX_USART1_UART_Init();                                      //USART Init
    UART1_SendStr("MX_USART1_UART_Init OK\r\n");
    
    MX_TIM1_Init();                                             //Tim1  Init
    MX_TIM6_Init();                                             //Tim6  Init 
    MX_IWDG_Init();                                             //Idog  Init
    while(1)
    { 	
/**************************************************************/
        if(Motor_ENmode_flag ==1){
            if(ENIN==1) {                            
                restart_init();                              
//                if(Reset_status_flag !=0 ){
//                //LL_TIM_DisableCounter(TIM1);
//                CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN); 
//                //LL_TIM_SetCounter(TIM1,0);
////                WRITE_REG(TIM1->CNT, 0);                     
//                  PID_Cal_value_init();           //
//                    
//                //LL_TIM_EnableCounter(TIM1);
//                SET_BIT(TIM1->CR1, TIM_CR1_CEN);
//                }
//                enmode=1;
//                Reset_status_flag=0;
            }
            else{
                Reset_status_flag++;     //0++
            }
        }else if(Motor_ENmode_flag ==0){
            if(ENIN==0){
                restart_init(); //               
//                if(Reset_status_flag !=0 ){         //
//                    
//                //LL_TIM_DisableCounter(TIM1);
//                CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN); 
//                //LL_TIM_SetCounter(TIM1,0);
////                WRITE_REG(TIM1->CNT, 0);   
//                    PID_Cal_value_init();           //
//                     
//                //LL_TIM_EnableCounter(TIM1);
//                    SET_BIT(TIM1->CR1, TIM_CR1_CEN);
//                }
//                enmode=1;                           // 
//                Reset_status_flag=0;
            }
            else{
                Reset_status_flag++;     //0++
            }
        }
/*******************************************************************/
        if(Reset_status_flag == 1){       
            enmode=0;
            Reset_status_flag ++;           //1++
            WRITE_REG(TIM3->CCR1, 0);
            WRITE_REG(TIM3->CCR2, 0);
            
            PID_Cal_value_init();           //
                
            wap1=0;
            wap2=0;

            Data_update_flag=1;
        }else{
            if(Reset_status_flag>3)
                Reset_status_flag--;
        }
        usart_Receive_Process();                        
        if(frame_Error_flag){
            frame_Error_flag =0;
             UART1_SendStr("Frame Err!\r\n");   
        }
        if(flash_store_flag ==1){
            flash_store_flag =0;

            NVIC_DisableIRQ(USART1_IRQn);
            STMFLASH_Write(Data_Store_Arrdess,table1,14);//
            NVIC_EnableIRQ(USART1_IRQn);
        }
        KeyScan();                                      
        Oled_display();                                 
        Motor_data_dis();                               

    }
}
//Restart init 
void restart_init(void)
{
    //
    if(Reset_status_flag !=0 ){
    //LL_TIM_DisableCounter(TIM1);
        CLEAR_BIT(TIM1->CR1, TIM_CR1_CEN); 
    //LL_TIM_SetCounter(TIM1,0);
//      WRITE_REG(TIM1->CNT, 0);                     
      PID_Cal_value_init();           //
        
 //LL_TIM_EnableCounter(TIM1);
        SET_BIT(TIM1->CR1, TIM_CR1_CEN);
    }
    enmode=1;
    Reset_status_flag=0;
}
//Key Scan 
void KeyScan(void)
{
    if(KEY_Select == 0){        //
        LL_mDelay(10);
        if(KEY_Select == 0){
            if(Menu_Num ==0){//
                if(KEY_Select_flag ==0){//
                    KEY_Select_flag=1;
                    Menu_update_flag=1;                 //
                    
                    if(Calibration_flag!=0xAA){         //
                        Menu_Num_item++;                //
                        if(Menu_Num_item>1)             //
                            Menu_Num_item=0;
                    }
                    else if(Second_Menu==1){
                            Menu_Num_item++;                //
                            if(Menu_Num_item>MENU_NUM){            //
                                Menu_Num_item =2;
                        }
                    }
                    else if(Second_Menu==2){                //
                            Menu_Num2_item++;
                            if(Menu_Num2_item>33)
                                Menu_Num2_item=0;
                    }
                    else if(Second_Menu==3){                //
                            Menu_Num3_item++;
                            if(Menu_Num3_item>4)
                                Menu_Num3_item=0;
                    }
                    else if(Second_Menu==4){                //
                            Menu_Num4_item++;
                            if(Menu_Num4_item>1)
                                Menu_Num4_item=0;
                    }
                    else if(Second_Menu==5){                //
                            Menu_Num5_item++;
                            if(Menu_Num5_item>1)
                                Menu_Num5_item=0;
                    }
//                    else if(Second_Menu==6){                //
//                            Menu_Num6_item++;
//                            if(Menu_Num6_item>1)
//                                Menu_Num6_item=0;
//                    }
                }
            }
        }
    }else if(KEY_Back == 0){
        LL_mDelay(10);
        if(KEY_Back == 0){
            if(KEY_Back_flag ==0){
                KEY_Back_flag=1;
                Menu_update_flag=1;         //
                OLED_Clear();                   //
                //
                if(Enter_exit_flag == 1){
                    Enter_exit_flag=0;
                    Menu_Num=0;                         //
                    Menu_Num_item=2;                    //
                    Second_Menu=1;
                    //break;
                }else {
                    Menu_Num++;
                    if(Menu_Num >1 ){
                        Menu_Num=0;
                    }
                }
            }
        }
    }else if(KEY_Confirm == 0){
        LL_mDelay(10);
        if(KEY_Confirm == 0){
            if(KEY_Confirm_flag ==0){
                KEY_Confirm_flag=1;
                switch(Menu_Num){
                    case 0:     
                        switch(Menu_Num_item){                  //
                            case 0:
                                    SetModeCheck();             
//                                    CalibrateEncoder_finish_flag=1; //
//                                    //写flash  
//                                    flash_store_flag =1;            //
//                                    table1[0] =0xAA;
//                                    Calibration_flag =0xAA;    //
//                                    Second_Menu=1;             // 
//                            
//                                    OLED_Clear();   //
//                                    OLED_ShowString(0,0,"->Calibrate");
//                                    OLED_ShowString(0,16,"  Motor mA");
//                                    OLED_ShowString(0,32,"  Microstep");
//                                    OLED_ShowString(0,48,"  EnablePin");
//                                    Menu_Num_item=2;                                //
                            break;
                            case 1: Enter_exit_flag=1;    
                                    Menu_Num=1;           // 
                                    Menu_update_flag=1;   //
                                    OLED_Clear();         //
                            break ;
                            case 2: 
                                    LL_TIM_DisableCounter(TIM1);
                                    enmode=0;                       ////
                                    NVIC_DisableIRQ(EXTI0_1_IRQn);
                                    NVIC_DisableIRQ(EXTI2_3_IRQn);
                                    Second_Calibrate_flag=1;                        //

                                    SetModeCheck();             
                                    NVIC_EnableIRQ(EXTI0_1_IRQn);
                                    NVIC_EnableIRQ(EXTI2_3_IRQn);
//                                    LL_TIM_EnableCounter(TIM1);
                                    Second_Menu=1;              //
                                break ;
                            case 3:
                                    OLED_Clear();   //
                                    Menu_update_flag=1;   //
                                    enter_num++;
                                    if(enter_num==1)
                                        Second_Menu=2;        //
                                    if(enter_num==2){
                                        enter_num=0;
                                        Second_Menu=1;
                                        //
                                        //
                                        Currents= Currents_Set;
                                        flash_store_flag =1;
                                        table1[1]=Currents;
                                        table1[2]=Menu_Num2_item;
                                    }
                                break ;
                            case 4: OLED_Clear();   //
                                    Menu_update_flag=1;   //
                                    enter_num++;
                                    if(enter_num==1)
                                        Second_Menu=3;        //
                                    if(enter_num==2){
                                        enter_num=0;
                                        Second_Menu=1;
                                        enmode =0;                      //
                                        PID_Cal_value_init();           /**/
                                        //
                                        stepangle =Microstep_Set;   //
                                        
                                        flash_store_flag =1;        //
                                        table1[3]=stepangle;        //
                                        table1[4]=Menu_Num3_item;
                                    }
                                break ;
                            case 5: OLED_Clear();   //
                                    Menu_update_flag=1;   //
                                    enter_num++;
                                    if(enter_num==1)
                                        Second_Menu=4;        //
                                    if(enter_num==2){
                                        enter_num=0;
                                        Second_Menu=1;
                                        enmode =0;                          //
                                        PID_Cal_value_init();               //
                                        //
                                        if(Dir_Enable == 0xaa ){
                                            //enmode =1;
                                          Motor_ENmode_flag=0;
                                        }else if(Dir_Enable == 0x55  ){
                                            Motor_ENmode_flag=1;
                                            //enmode =0;
                                            //LL_TIM_OC_SetCompareCH1(TIM3,0);  
                                            //LL_TIM_OC_SetCompareCH2(TIM3,0);  
                                        }
                                        if(Dir_Enable==0x55 || Dir_Enable ==0xAA){
                                            Dir_Enable =0;
                                            flash_store_flag =1;//
                                            table1[5]=Motor_ENmode_flag;        //
                                            table1[6]=Menu_Num4_item;
                                            
                                        }
                                    }
                                break ;
                            case 6: OLED_Clear();   //
                                    Menu_update_flag=1;   //
                                    enter_num++;
                                    if(enter_num==1)
                                        Second_Menu=5;        //
                                    if(enter_num==2){
                                        enter_num=0;
                                        Second_Menu=1;
                                        
                                        //
                                        //Dir_Enable  
                                        if(Dir_Enable ==0x11){
                                            Motor_Dir =1;
                                        }else if(Dir_Enable == 0x22){
                                            Motor_Dir=0;
                                        }
                                        //if(Dir_Enable ==0x11 || Dir_Enable==0x22){
                                            Dir_Enable=0;
                                        //}
                                        flash_store_flag =1;//
                                        table1[7]=Motor_Dir;                    //
                                        table1[8]=Menu_Num5_item;
                                    }
                                break ;
                                    
//                            case 7:OLED_Clear();   //
//                                    Menu_update_flag=1;   //
//                                    enter_num++;
//                                    if(enter_num==1)
//                                        Second_Menu=6;        //
//                                    if(enter_num==2){
//                                        enter_num=0;
//                                        Second_Menu=1;
//                                        
//                                        //Dir_Enable  
//                                        if(Dir_Enable ==0x33){
//                                            Motor_mode =0;
//                                            closemode=1;        //
//                                        }else if(Dir_Enable == 0x44){
//                                            Motor_mode=1;
//                                            closemode=0;        //
//                                        }
//                                        
//                                        Dir_Enable=0;
////                                        flash_store_flag =1;        //
////                                        table1[9]=Motor_mode;        //
////                                        table1[10]=Menu_Num6_item;
//                                    }
//                                break ;
                            case 7:Enter_exit_flag=1;    
                                    Menu_Num=1;           //  
                                    Menu_update_flag=1;   //
                                    OLED_Clear();         //
                                break ;
                            default:break ;
                        }
                    break ;
//                            case 1: Enter_exit_flag=1;    
//                                
//                            break ;
                }
            }
        }
    }else{
        KEY_Select_flag=0;       //
        KEY_Back_flag=0;         //
        KEY_Confirm_flag=0;
    }
}

static void LL_Init(void)
{
  LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SYSCFG);
  NVIC_SetPriority(SVC_IRQn, 0);
  NVIC_SetPriority(PendSV_IRQn, 0);
  NVIC_SetPriority(SysTick_IRQn, 0);
}


void SystemClock_Config(void)
{
  LL_FLASH_SetLatency(LL_FLASH_LATENCY_1);
	
  if(LL_FLASH_GetLatency() != LL_FLASH_LATENCY_1)
  {
    Error_Handler();  
  }
  LL_RCC_HSI_Enable();
  
  while(LL_RCC_HSI_IsReady() != 1)
  {}
  LL_RCC_HSI_SetCalibTrimming(16);
  LL_RCC_LSI_Enable();
  while(LL_RCC_LSI_IsReady() != 1)
  {}
  LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI_DIV_2, LL_RCC_PLL_MUL_12);
  LL_RCC_PLL_Enable();
	  
  while(LL_RCC_PLL_IsReady() != 1)
  {}
  LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
  LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1);
  LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);
	  
  while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL)
  {}
  LL_Init1msTick(48000000);
  LL_SYSTICK_SetClkSource(LL_SYSTICK_CLKSOURCE_HCLK);
  LL_SetSystemCoreClock(48000000);
  LL_RCC_SetUSARTClockSource(LL_RCC_USART1_CLKSOURCE_PCLK1);
  NVIC_SetPriority(SysTick_IRQn, 0);
}

static void MX_IWDG_Init(void)
{
  LL_IWDG_Enable(IWDG);
  LL_IWDG_EnableWriteAccess(IWDG);
  LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_256);
  LL_IWDG_SetWindow(IWDG, 4095);
  LL_IWDG_SetReloadCounter(IWDG, 0xfff);
  while (LL_IWDG_IsReady(IWDG) != 1)
  {}
  LL_IWDG_ReloadCounter(IWDG);
}
//
static void MX_SPI1_Init(void)
{
  LL_SPI_InitTypeDef SPI_InitStruct;
  LL_GPIO_InitTypeDef GPIO_InitStruct;
	
  LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_SPI1);
  
  /**SPI1 GPIO Configuration  
  PA4   ------> SPI1_NSS
  PA5   ------> SPI1_SCK
  PA6   ------> SPI1_MISO
  PA7   ------> SPI1_MOSI 
  */

  GPIO_InitStruct.Pin = LL_GPIO_PIN_5;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_0;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_6;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_0;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_7;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_DOWN;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_0;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  SPI_InitStruct.TransferDirection = LL_SPI_FULL_DUPLEX;
  SPI_InitStruct.Mode = LL_SPI_MODE_MASTER;
  SPI_InitStruct.DataWidth = LL_SPI_DATAWIDTH_16BIT;
  SPI_InitStruct.ClockPolarity = LL_SPI_POLARITY_LOW;
  SPI_InitStruct.ClockPhase = LL_SPI_PHASE_2EDGE;
  SPI_InitStruct.NSS = LL_SPI_NSS_SOFT;
  SPI_InitStruct.BaudRate = LL_SPI_BAUDRATEPRESCALER_DIV8;
  SPI_InitStruct.BitOrder = LL_SPI_MSB_FIRST;
  SPI_InitStruct.CRCCalculation = LL_SPI_CRCCALCULATION_DISABLE;
  SPI_InitStruct.CRCPoly = 7;
  LL_SPI_Init(SPI1, &SPI_InitStruct);
  LL_SPI_SetStandard(SPI1, LL_SPI_PROTOCOL_MOTOROLA); 
  LL_SPI_Enable(SPI1); 
}
//
static void MX_TIM1_Init(void)
{
  LL_TIM_InitTypeDef TIM_InitStruct = {0};
  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* Peripheral clock enable */
  LL_APB1_GRP2_EnableClock(LL_APB1_GRP2_PERIPH_TIM1);
  
  LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
  /**TIM1 GPIO Configuration  
  PA12   ------> TIM1_ETR 
  */
  GPIO_InitStruct.Pin = LL_GPIO_PIN_12;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_2;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);


  TIM_InitStruct.Prescaler = 0;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = 0xFFFF;
  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
  TIM_InitStruct.RepetitionCounter = 0;
  LL_TIM_Init(TIM1, &TIM_InitStruct);
  LL_TIM_DisableARRPreload(TIM1);
  LL_TIM_ConfigETR(TIM1, LL_TIM_ETR_POLARITY_NONINVERTED, LL_TIM_ETR_PRESCALER_DIV1, LL_TIM_ETR_FILTER_FDIV4_N8);
  LL_TIM_SetClockSource(TIM1, LL_TIM_CLOCKSOURCE_EXT_MODE2);
  LL_TIM_SetTriggerOutput(TIM1, LL_TIM_TRGO_RESET);
  LL_TIM_DisableMasterSlaveMode(TIM1);
  LL_TIM_EnableCounter(TIM1);
}
//
static void MX_TIM3_Init(void)
{
  LL_TIM_InitTypeDef TIM_InitStruct;
  LL_TIM_OC_InitTypeDef TIM_OC_InitStruct;
  LL_GPIO_InitTypeDef GPIO_InitStruct;

  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM3);
	
  /**TIM3 GPIO Configuration  
  PB4   ------> TIM3_CH1
  PB5   ------> TIM3_CH2 
  */
  GPIO_InitStruct.Pin = PWM1_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_1;
  LL_GPIO_Init(PWM1_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = PWM2_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_1;
  LL_GPIO_Init(PWM2_GPIO_Port, &GPIO_InitStruct);

  TIM_InitStruct.Prescaler = 0;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = 256;//
  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
  LL_TIM_Init(TIM3, &TIM_InitStruct);

  LL_TIM_DisableARRPreload(TIM3);

  LL_TIM_SetClockSource(TIM3, LL_TIM_CLOCKSOURCE_INTERNAL);

  LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH1);

  TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1;
  TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
  TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE;
  TIM_OC_InitStruct.CompareValue = 0;
  TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
  LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);

  LL_TIM_OC_DisableFast(TIM3, LL_TIM_CHANNEL_CH1);

  LL_TIM_OC_EnablePreload(TIM3, LL_TIM_CHANNEL_CH2);

  TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
  TIM_OC_InitStruct.OCNState = LL_TIM_OCSTATE_DISABLE;
  LL_TIM_OC_Init(TIM3, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);

  LL_TIM_OC_DisableFast(TIM3, LL_TIM_CHANNEL_CH2);

  LL_TIM_SetTriggerOutput(TIM3, LL_TIM_TRGO_RESET);

  LL_TIM_DisableMasterSlaveMode(TIM3);
  
  LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH1);
  LL_TIM_CC_EnableChannel(TIM3, LL_TIM_CHANNEL_CH2);
  LL_TIM_EnableCounter(TIM3);
}
//
static void MX_TIM6_Init(void)
{
  LL_TIM_InitTypeDef TIM_InitStruct;

  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM6);
  NVIC_SetPriority(TIM6_IRQn, 1);
  NVIC_EnableIRQ(TIM6_IRQn);

  TIM_InitStruct.Prescaler = 0;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = 4800;//
  LL_TIM_Init(TIM6, &TIM_InitStruct);

  LL_TIM_DisableARRPreload(TIM6);
	
  LL_TIM_EnableIT_UPDATE(TIM6);
  LL_TIM_EnableCounter(TIM6);
}

static void MX_GPIO_Init(void)
{
  LL_GPIO_InitTypeDef GPIO_InitStruct;
  LL_EXTI_InitTypeDef EXTI_InitStruct;

  LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOC);
  LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOA);
  LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);

  LL_GPIO_ResetOutputPin(LED_GPIO_Port, LED_Pin);
  LL_GPIO_ResetOutputPin(IN1_GPIO_Port, IN1_Pin);
  LL_GPIO_ResetOutputPin(IN2_GPIO_Port, IN2_Pin);
  LL_GPIO_ResetOutputPin(IN4_GPIO_Port, IN4_Pin);
  LL_GPIO_ResetOutputPin(IN3_GPIO_Port, IN3_Pin);

/*****************************************/
  GPIO_InitStruct.Pin = CAL_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(CAL_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = CLOSE_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(CLOSE_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = SET2_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(SET2_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = SET1_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(SET1_GPIO_Port, &GPIO_InitStruct);
/*****************************************************/
  GPIO_InitStruct.Pin = KEY_Select_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(KEY_Select_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = KEY_Back_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(KEY_Back_GPIO_Port, &GPIO_InitStruct);   
  
  GPIO_InitStruct.Pin = KEY_Confirm_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  LL_GPIO_Init(KEY_Confirm_GPIO_Port, &GPIO_InitStruct); 
  
/***********************************************/  
/***************oled IO**************************************/
  GPIO_InitStruct.Pin = OLED_CS_Pin| OLED_RS_Pin |OLED_SCLK_Pin|OLED_SDIN_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(GPIOB, &GPIO_InitStruct);
  
  GPIO_InitStruct.Pin = OLED_RST_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(OLED_RST_GPIO_Port, &GPIO_InitStruct);
  
/*************************************************/
  GPIO_InitStruct.Pin = LED_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(LED_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = IN1_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(IN1_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = IN2_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(IN2_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = IN4_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(IN4_GPIO_Port, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = IN3_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(IN3_GPIO_Port, &GPIO_InitStruct);
  
  GPIO_InitStruct.Pin = NSS_Pin;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  LL_GPIO_Init(NSS_GPIO_Port, &GPIO_InitStruct);

  LL_SYSCFG_SetEXTISource(LL_SYSCFG_EXTI_PORTB, LL_SYSCFG_EXTI_LINE1);
  LL_SYSCFG_SetEXTISource(LL_SYSCFG_EXTI_PORTB, LL_SYSCFG_EXTI_LINE2);
  
  LL_GPIO_SetPinPull(DIRIN_GPIO_Port, DIRIN_Pin, LL_GPIO_PULL_NO);//
  LL_GPIO_SetPinPull(ENIN_GPIO_Port, ENIN_Pin, LL_GPIO_PULL_UP);//
  LL_GPIO_SetPinMode(DIRIN_GPIO_Port, DIRIN_Pin, LL_GPIO_MODE_INPUT);
  LL_GPIO_SetPinMode(ENIN_GPIO_Port, ENIN_Pin, LL_GPIO_MODE_INPUT);

  EXTI_InitStruct.Line_0_31 = LL_EXTI_LINE_1;
  EXTI_InitStruct.LineCommand = ENABLE;
  EXTI_InitStruct.Mode = LL_EXTI_MODE_IT;
  EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING_FALLING;
  LL_EXTI_Init(&EXTI_InitStruct);

//  EXTI_InitStruct.Line_0_31 = LL_EXTI_LINE_2;
//  EXTI_InitStruct.LineCommand = ENABLE;
//  EXTI_InitStruct.Mode = LL_EXTI_MODE_IT;
//  EXTI_InitStruct.Trigger = LL_EXTI_TRIGGER_RISING_FALLING;
//  LL_EXTI_Init(&EXTI_InitStruct);

  NVIC_SetPriority(EXTI0_1_IRQn, 0);
  NVIC_DisableIRQ(EXTI0_1_IRQn);
//  NVIC_SetPriority(EXTI2_3_IRQn, 1);
//  NVIC_DisableIRQ(EXTI2_3_IRQn);
}

void SetModeCheck(void)
{
  WriteValue(WRITE_MOD2_VALUE,MOD2_VALUE);
  uint16_t state=ReadState();//
  if(state&0x0080)//
  {
    for(uint8_t m=0;m<10;m++){
      LED_H;
	  LL_mDelay(200);
	  LED_L;
	  LL_mDelay(200);	
    } 
  }	
  if(Calibration_flag !=0xAA){
loop: if(CAL==0)
        CalibrateEncoder();
        if(1 != Second_Calibrate_flag){
          if((SET1==1)&&(SET2==1))//
            stepangle=16;//
          else if((SET1==0)&&(SET2==1))
            stepangle=8;//
          else if((SET1==1)&&(SET2==0))
            stepangle=4;//
          else
            stepangle=2;//
        }
    }
    else if(Calibration_flag == 0xAA && Second_Calibrate_flag ==1){
//      Second_Calibrate_flag=0;
      goto loop;
    }
    if(CLOSE==0 )//|| Motor_mode==0
    {//
        closemode=1;
    #if 1       
        r=*(volatile uint16_t*)((ReadValue(READ_ANGLE_VALUE)>>1)*2+0x08008000); 
        s_sum=r;   //
        y=r;
        y_1=y;
        yw=y;  
        yw_1=yw;
    #endif
    }
    else{
        closemode=0;
    }

    if(CalibrateEncoder_finish_flag ==1){   
        CalibrateEncoder_finish_flag=0; 
        Second_Calibrate_flag=0;
        Prompt_show();               //
        for(;;){
            LED_F;
            LL_mDelay(200);
        }
    }
//    else{
//        NVIC_EnableIRQ(EXTI0_1_IRQn);
//        NVIC_EnableIRQ(EXTI2_3_IRQn);
//    }
}

void Output(int32_t theta,uint8_t effort) 
{	
  int16_t v_coil_A;
  int16_t v_coil_B;

  int16_t sin_coil_A;
  int16_t sin_coil_B;
	
  int16_t angle_1;
  int16_t angle_2;
		
  float phase_multiplier=12.5f;//

  angle_1=Mod(phase_multiplier*theta,4096);//
  angle_2=angle_1+1024;
  if(angle_2>4096)
    angle_2-=4096;

  sin_coil_A=sin_1[angle_1];
  sin_coil_B=sin_1[angle_2];

  v_coil_A=effort*sin_coil_A/1024;//
  v_coil_B=effort*sin_coil_B/1024;//
	
  if(v_coil_A>=0)  
  {
    LL_TIM_OC_SetCompareCH2(TIM3,v_coil_A);  
	IN1_HIGH;  
    IN2_LOW;  
  }
  else  
  {
    LL_TIM_OC_SetCompareCH2(TIM3,-v_coil_A);  
	IN1_LOW;     
    IN2_HIGH;  
  } 
  if(v_coil_B>=0)  
  {
    LL_TIM_OC_SetCompareCH1(TIM3,v_coil_B);  
	IN3_HIGH;  
    IN4_LOW;  
  }
  else 
  {
    LL_TIM_OC_SetCompareCH1(TIM3,-v_coil_B); 
	IN3_LOW;     
    IN4_HIGH;    
  }
}
//
void OneStep(void)
{          
  if(dir) 
    stepnumber+=1;
  else 
    stepnumber-=1;
  
  Output(81.92f*stepnumber,80);
  LL_mDelay(10);
}
//
int16_t Mod(int32_t xMod,int16_t mMod) 
{
  int16_t temp;
  temp=xMod%mMod;
  if(temp<0)
    return (temp+mMod);
  else
	return  temp;
}

void UsDelay(uint16_t us)
{
  us*=10;
  while(us)	
  {
    __NOP();
	__NOP();
	us--;
  }
}
//
void FlashUnlock(void)
{
  FLASH->KEYR=FLASH_KEY1;
  FLASH->KEYR=FLASH_KEY2;
}
//
void FlashLock(void)
{
  FLASH->CR|=1<<7; 
}
//
uint8_t FlashGetStatus(void)
{
  uint32_t res;
  res=FLASH->SR;
  if(res&(1<<0))//flash busy
    return 1; 
  else if(res&(1<<2))  //Programming error
	return 2; 
  else if(res&(1<<4))  //
	return 3; 
  return 0;     
}

uint8_t FlashWaitDone(uint16_t time)
{
  uint8_t res;
  do
  {
    res=FlashGetStatus();
    if(res!=1)
	  break;
	UsDelay(1);	
    time--;
  }while(time);
  if(time==0)
    res=0xff;
  return res;
}
//
uint8_t FlashErasePage(uint32_t paddr)
{
  uint8_t res=0;
  res=FlashWaitDone(0X5FFF);
  if(res==0)
  {
    FLASH->CR|=1<<1; 
    FLASH->AR=paddr; 
    FLASH->CR|=1<<6; 
    res=FlashWaitDone(0X5FFF); 
    if(res!=1) 
      FLASH->CR&=~(1<<1); 
  }
  return res;
}
//
void FlashErase32K(void)
{
  FlashErasePage(0x08008000);
  FlashErasePage(0x08008400);
  FlashErasePage(0x08008800);
  FlashErasePage(0x08008C00);
  FlashErasePage(0x08009000);
  FlashErasePage(0x08009400);
  FlashErasePage(0x08009800);
  FlashErasePage(0x08009C00);
  FlashErasePage(0x0800A000);
  FlashErasePage(0x0800A400);
  FlashErasePage(0x0800A800);
  FlashErasePage(0x0800AC00);
  FlashErasePage(0x0800B000);
  FlashErasePage(0x0800B400);
  FlashErasePage(0x0800B800);
  FlashErasePage(0x0800BC00);
  FlashErasePage(0x0800C000);
  FlashErasePage(0x0800C400);
  FlashErasePage(0x0800C800);
  FlashErasePage(0x0800CC00);
  FlashErasePage(0x0800D000);
  FlashErasePage(0x0800D400);
  FlashErasePage(0x0800D800);
  FlashErasePage(0x0800DC00);
  FlashErasePage(0x0800E000);
  FlashErasePage(0x0800E400);
  FlashErasePage(0x0800E800);
  FlashErasePage(0x0800EC00);
  FlashErasePage(0x0800F000);
  FlashErasePage(0x0800F400);
  FlashErasePage(0x0800F800); 
  FlashErasePage(0x0800FC00);
}
//
uint8_t FlashWriteHalfWord(uint32_t faddr,uint16_t dat)
{
  uint8_t  res;
  res=FlashWaitDone(0XFF);
  if(res==0)
  {
    FLASH->CR|=1<<0;
    *(volatile uint16_t*)faddr=dat; 
    res=FlashWaitDone(0XFF);
	if(res!=1)
    {
      FLASH->CR&=~(1<<0); 
    }
  }
  return res;
}
//
uint16_t FlashReadHalfWord(uint32_t faddr)
{
  return *(volatile uint16_t*)faddr;
}
           
//
uint16_t ReadValue(uint16_t RegAdd)
{
  uint16_t data;
  NSS_L;
  while(LL_SPI_IsActiveFlag_TXE(SPI1)==0);
  LL_SPI_TransmitData16(SPI1,RegAdd);
  while(LL_SPI_IsActiveFlag_RXNE(SPI1)==0);
  data=LL_SPI_ReceiveData16(SPI1);
  SPI_TX_OD;
  while(LL_SPI_IsActiveFlag_TXE(SPI1)==0);
  LL_SPI_TransmitData16(SPI1,0xFFFF);
  while(LL_SPI_IsActiveFlag_RXNE(SPI1)==0);
  data=LL_SPI_ReceiveData16(SPI1)&0x7FFF;
  NSS_H;
  SPI_TX_PP;
  return data;
}
//
void WriteValue(uint16_t RegAdd,uint16_t RegValue)
{
  NSS_L;
  while(LL_SPI_IsActiveFlag_TXE(SPI1)==0);
  LL_SPI_TransmitData16(SPI1,RegAdd);
  while(LL_SPI_IsActiveFlag_RXNE(SPI1)==0);
  LL_SPI_ReceiveData16(SPI1);
  while(LL_SPI_IsActiveFlag_TXE(SPI1)==0);
  LL_SPI_TransmitData16(SPI1,RegValue);
  while(LL_SPI_IsActiveFlag_RXNE(SPI1)==0);
  LL_SPI_ReceiveData16(SPI1);
  NSS_H;
}
//
uint16_t ReadState(void)
{
  return (ReadValue(READ_STATUS));
}
//
uint16_t ReadAngle(void)
{
  return (ReadValue(READ_ANGLE_VALUE)>>1);
}
//
void CalibrateEncoder(void) 
{   
  int32_t encoderReading=0;    
  int32_t currentencoderReading=0;
  int32_t lastencoderReading=0;        

  int32_t iStart=0;    
  int32_t jStart=0;
  int32_t stepNo=0;
  
  int32_t fullStepReadings[200];//
  int32_t ticks=0;	
  uint32_t address=0x08008000;//

  uint16_t lookupAngle;
		
  dir=1; 
  Output(0,80);//
  for(uint8_t m=0;m<4;m++)
  {
    LED_H;
	LL_mDelay(250);
    LED_L;
	LL_mDelay(250);	
  } 
  for(int16_t x=0;x<=199;x++)//
  {    
    encoderReading=0;
   	LL_mDelay(20);                     
    lastencoderReading=ReadAngle();     
    for(uint8_t reading=0;reading<10;reading++) 
	{ 
      currentencoderReading=ReadAngle(); 
      if(currentencoderReading-lastencoderReading<-8192)
        currentencoderReading+=16384;
      else if(currentencoderReading-lastencoderReading>8192)
        currentencoderReading-=16384;
 
      encoderReading+=currentencoderReading;
      LL_mDelay(10);
      lastencoderReading=currentencoderReading;
    }
    encoderReading=encoderReading/10;
    if(encoderReading>16384)
      encoderReading-=16384;
    else if(encoderReading<0)
      encoderReading+=16384;
    fullStepReadings[x]=encoderReading;  
    OneStep();
	LL_mDelay(100); 
  }
  dir=0; 
  OneStep();
  LL_mDelay(1000); 
  for(int16_t x=199;x>=0;x--)//
  {    
    encoderReading=0;
   	LL_mDelay(20);                     
    lastencoderReading=ReadAngle();     
    for(uint8_t reading=0;reading<10;reading++) 
	{ 
      currentencoderReading=ReadAngle(); 
      if(currentencoderReading-lastencoderReading<-8192)
        currentencoderReading+=16384;
      else if(currentencoderReading-lastencoderReading>8192)
        currentencoderReading-=16384;
 
      encoderReading+=currentencoderReading;
      LL_mDelay(10);
      lastencoderReading=currentencoderReading;
    }
    encoderReading=encoderReading/10;
    if(encoderReading>16384)
      encoderReading-=16384;
    else if(encoderReading<0)
      encoderReading+=16384;
    fullStepReadings[x]=(fullStepReadings[x]+encoderReading)/2;  
    OneStep();
	LL_mDelay(100); 
  }
  LL_TIM_OC_SetCompareCH1(TIM3,0);  
  LL_TIM_OC_SetCompareCH2(TIM3,0); 
  for(uint8_t i=0;i<200;i++)//
  {
    ticks=fullStepReadings[(i+1)%200]-fullStepReadings[i%200];
    if(ticks<-15000) 
      ticks+=16384;
    else if(ticks>15000)	
	  ticks-=16384;	
    for(int32_t j=0;j<ticks;j++) 
	{
      stepNo=(fullStepReadings[i]+j)%16384;
      if(stepNo==0) 
      {
        iStart=i;
        jStart=j;
      }
    }
  }
  FlashUnlock();
  FlashErase32K();
  for(int32_t i=iStart;i<(iStart+200+1);i++)//
  {
	ticks=fullStepReadings[(i+1)%200]-fullStepReadings[i%200];
    if(ticks<-15000) 
      ticks+=16384;         
    if(i==iStart) 
	{ 
      for(int32_t j=jStart;j<ticks;j++) 
	  {
        lookupAngle=(8192*i+8192*j/ticks)%1638400/100;
		FlashWriteHalfWord(address,(uint16_t)lookupAngle);
		address+=2;
      }
    }
    else if(i==(iStart+200)) 
	{ 
      for(int32_t j=0;j<jStart;j++) 
	  {
        lookupAngle=((8192*i+8192*j/ticks)%1638400)/100;
		FlashWriteHalfWord(address,(uint16_t)lookupAngle);
		address+=2;
      }
    }
    else 
	{                        //this is the general case
      for(int32_t j=0;j<ticks;j++) 
      {
        lookupAngle=((8192*i+8192*j/ticks)%1638400)/100;
		FlashWriteHalfWord(address,(uint16_t)lookupAngle);
		address+=2;
      }
    }
  }
  FlashLock();
  if(Second_Calibrate_flag !=1){                            //
    //
    flash_store_flag =1;            //
    table1[0] =0xAA;                //
    table1[1] =128;table1[2] =16;
    table1[3] =4;table1[4] =3;
    table1[5] =0;table1[6] =1;
    table1[7] =1;table1[8] =1;
    table1[11]=kp;                  //
    table1[12]=ki;
    table1[13]=kd;
  
    Calibration_flag =0xAA;    //
//    Second_Menu=1;             // 
    STMFLASH_Write(Data_Store_Arrdess,table1,14);//
  }
  
  CalibrateEncoder_finish_flag=1; //  
}
//
void Prompt_show(void)
{
    OLED_Clear();
    //OLED_ShowString(0,0,"              ");
    OLED_ShowString(0,16,"   finished!  ");
    OLED_ShowString(0,32,"  Please press ");
    OLED_ShowString(0,48,"Reset Key reboot");
}
void _Error_Handler(char *file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  while(1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT

void assert_failed(uint8_t* file, uint32_t line)
{ 
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/**
  * @}
  */

/**
  * @}
  */

